///////////////////////////////////////////////////////////////////////////////
// FILE:          DemoCamera.cpp
// PROJECT:       Micro-Manager
// SUBSYSTEM:     DeviceAdapters
//-----------------------------------------------------------------------------
// DESCRIPTION:   The example implementation of the demo camera.
//                Simulates generic digital camera and associated automated
//                microscope devices and enables testing of the rest of the
//                system without the need to connect to the actual hardware. 
//                
// AUTHOR:        Nenad Amodaj, nenad@amodaj.com, 06/08/2005
//
// COPYRIGHT:     University of California, San Francisco, 2006
// LICENSE:       This file is distributed under the BSD license.
//                License text is included with the source distribution.
//
//                This file is distributed in the hope that it will be useful,
//                but WITHOUT ANY WARRANTY; without even the implied warranty
//                of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
//
//                IN NO EVENT SHALL THE COPYRIGHT OWNER OR
//                CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
//                INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES.
// CVS:           $Id: DemoCamera.cpp 3785 2010-01-16 06:21:07Z nico $
//

#include "GCode.h"
#include <cstdio>
#include <string>
#include <math.h>
#include "../../MMDevice/ModuleInterface.h"
#include "../../MMCore/Error.h"
#include <sstream>
using namespace std;

// this is for the stage, but why?
double g_IntensityFactor_ = 1.0;

// External names used used by the rest of the system
// to load particular device from the "GCode.dll" library
const char* g_StageDeviceName = "GCodeZAxis";
const char* g_XYStageDeviceName = "GCodeXYStage";


// TODO: linux entry code

// windows DLL entry code
#ifdef WIN32
BOOL APIENTRY DllMain( HANDLE /*hModule*/, 
                      DWORD  ul_reason_for_call, 
                      LPVOID /*lpReserved*/
                      )
{
   switch (ul_reason_for_call)
   {
   case DLL_PROCESS_ATTACH:
   case DLL_THREAD_ATTACH:
   case DLL_THREAD_DETACH:
   case DLL_PROCESS_DETACH:
      break;
   }
   return TRUE;
}
#endif

///////////////////////////////////////////////////////////////////////////////
// Exported MMDevice API
///////////////////////////////////////////////////////////////////////////////

/**
 * List all supported hardware devices here
 * Do not discover devices at runtime.  To avoid warnings about missing DLLs, Micro-Manager
 * maintains a list of supported device (MMDeviceList.txt).  This list is generated using 
 * information supplied by this function, so runtime discovery will create problems.
 */
MODULE_API void InitializeModuleData()
{
   AddAvailableDeviceName(g_StageDeviceName, "GCode Z Axis");
   AddAvailableDeviceName(g_XYStageDeviceName, "GCode XY stage");
}

MODULE_API MM::Device* CreateDevice(const char* deviceName)
{
   if (deviceName == 0)
      return 0;
   // decide which device class to create based on the deviceName parameter
   if (strcmp(deviceName, g_StageDeviceName) == 0)
   {
      // create stage - Z axis
      return new GCodeZAxis(); //GCodeZAxis GCodeXYStage
   }
   else if (strcmp(deviceName, g_XYStageDeviceName) == 0)
   {
      // create XY stage
      return new GCodeXYStage();
   }
   // ...supplied name not recognized
   return 0;
}

MODULE_API void DeleteDevice(MM::Device* pDevice)
{
   delete pDevice;
}


///////////////////////////////////////////////////////////////////////////////
//  GCodeZAxis implementation
//    
// ~~~~~~~~~~~~~~~~~~~~~~~~~
GCodeZAxis::GCodeZAxis() :  
port_("Undefined"),
answerTimeoutMs_(1000),
stepSize_Um_(1.0),
pos_um_(0.0),
busy_(false),
initialized_(false),
lowerLimit_(0.0),
upperLimit_(20000.0)
{
   InitializeDefaultErrorMessages();
  
   // create pre-initialization properties
   // ------------------------------------

 
   // Port
   CPropertyAction* pAct = new CPropertyAction (this, &GCodeZAxis::OnPort);
   CreateProperty(MM::g_Keyword_Port, "Undefined", MM::String, false, pAct, true);

}

GCodeZAxis::~GCodeZAxis()
{
   Shutdown();
}

void GCodeZAxis::GetName(char* Name) const
{
   CDeviceUtils::CopyLimitedString(Name, g_StageDeviceName);
}

int GCodeZAxis::Initialize()
{

   LogMessage("GCodeZAxis Initialize false", false);

   if (initialized_)
      return DEVICE_OK;

   // set property list
   // -----------------
   curSteps_ =0;
   // Name
   int ret = CreateProperty(MM::g_Keyword_Name, g_StageDeviceName, MM::String, true);
   if (DEVICE_OK != ret)
      return ret;

   // Description
   ret = CreateProperty(MM::g_Keyword_Description, "GCode Z Axis Driver", MM::String, true);
   if (DEVICE_OK != ret)
      return ret;

   // Position
   // --------
   CPropertyAction* pAct = new CPropertyAction (this, &GCodeZAxis::OnPosition);
   ret = CreateProperty(MM::g_Keyword_Position, "0", MM::Float, false, pAct);
   if (ret != DEVICE_OK)
      return ret;

   ret = GetPositionSteps(curSteps_);
   
   if (ret != DEVICE_OK)
      return ret;
   curSteps_ =0;

   // StepSize
   CPropertyAction* pAct2 = new CPropertyAction (this, &GCodeZAxis::OnStepSizeUm);
   CreateProperty("StepSizeUm", "1.0", MM::Float, false, pAct2);
   stepSize_Um_ = 1.0;

   ret = UpdateStatus();
   if (ret != DEVICE_OK)
      return ret;

   initialized_ = true;

   return DEVICE_OK;
}

int GCodeZAxis::Shutdown()
{
   if (initialized_)
   {
      initialized_ = false;
   }
   return DEVICE_OK;
}

void GCodeZAxis::SetIntensityFactor(double pos)
{
   pos = fabs(pos);
   pos = 10.0 - pos;
   if (pos < 0)
      g_IntensityFactor_ = 1.0;
   else
      g_IntensityFactor_ = pos/10.0;
}


///////////////////////////////////////////////////////////////////////////////
// Action handlers
///////////////////////////////////////////////////////////////////////////////

int GCodeZAxis::OnPosition(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      // nothing to do, let the caller use cached property
   }
   else if (eAct == MM::AfterSet)
   {
      double pos;
      pProp->Get(pos);
      if (pos > upperLimit_ || lowerLimit_ > pos)
      {
         pProp->Set(pos_um_); // revert
         return ERR_UNKNOWN_POSITION;
      }
      pos_um_ = pos;
      //SetIntensityFactor(pos);
   }

   return DEVICE_OK;
}

int GCodeZAxis::OnPort(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      pProp->Set(port_.c_str());
   }
   else if (eAct == MM::AfterSet)
   {
      if (initialized_)
      {
         // revert
         pProp->Set(port_.c_str());
         return ERR_PORT_CHANGE_FORBIDDEN;
      }

      pProp->Get(port_);
   }

   return DEVICE_OK;
}

int GCodeZAxis::OnStepSizeUm(MM::PropertyBase* pProp, MM::ActionType eAct)
{
   if (eAct == MM::BeforeGet)
   {
      pProp->Set(stepSize_Um_);
   }
   else if (eAct == MM::AfterSet)
   {
      pProp->Get(stepSize_Um_);
   }

   return DEVICE_OK;
}


int GCodeZAxis::SetPositionUm(double pos)
{
   long steps = (long) (pos / stepSize_Um_ + 0.5);
   return SetPositionSteps(steps);
}

int GCodeZAxis::GetPositionUm(double& pos)
{
   long steps;
   int ret = GetPositionSteps(steps);
   if (ret != DEVICE_OK)
      return ret;
   pos = steps * stepSize_Um_;
   // LogMessage("GetPositionUm", false); 
   return DEVICE_OK;
}
  
int GCodeZAxis::SetPositionSteps(long pos)
{
   LogMessage("SetPositionSteps start", false);

   // what is pos?  are we in absolute or relative? wtf?

   long save_pos = pos;
   ostringstream command;
   command << "G00 Z" << pos;

   //log the command.
   char buffer[99];
   int i;
   i = sprintf(buffer, "SetPositionSteps pos: %d command: %s", pos, command.str().c_str());
   LogMessage(buffer, false);

   
   int ret = SendSerialCommand(port_.c_str(), command.str().c_str(), "\n");
   if (ret != DEVICE_OK){
	   i = sprintf(buffer, "SetPositionSteps SendSerialCommand not DEVICE_OK, ret: %i ", pos);
	   LogMessage(buffer, false);    
	   return ret;
   }

   
   //TODO: do proper blocking wait for 'R' from arduino.

   // block/wait for acknowledge, or until we time out;
   string answer;
   ret = GetSerialAnswer(port_.c_str(), "\n", answer);

   i = sprintf(buffer, "SetPositionSteps answer: %s", answer.c_str());
   LogMessage(buffer, false);
 
   if (ret != DEVICE_OK){
	  LogMessage("SetPositionSteps failed to get an answer from the USB port.  Whoops.", false);
	  return ret;
   }

   if (answer.substr(0, 1).compare("R") == 0)
   {
      curSteps_ = pos;
      return DEVICE_OK;
   }
   else 
   {
	  i = sprintf(buffer, "SetPositionSteps got |%s| from USB should have been 'R'", answer.c_str());
	  LogMessage("SetPositionSteps got |%s| from USB should have been 'R'", false);
  	  LogMessage("SetPositionSteps failed to get an answer from the USB port.  Whoops.", false);

	  int errNo = atoi(answer.substr(2).c_str());
      return ERR_OFFSET + errNo;
   }

   return ERR_UNRECOGNIZED_ANSWER;   
}
  
int GCodeZAxis::GetPositionSteps(long& steps)
{
   const char* command="M114";
   
   // send command - we want \n
   // clear port?  does this exist somehow? clearPort(*this, *GetCoreCallback(),port_.c_str());
   PurgeComPort(port_.c_str());
   int ret = SendSerialCommand(port_.c_str(), command, "\n");
  

   LogMessage("GetPositionSteps after SendSerialCommand", false);
   if (ret != DEVICE_OK)
      return ret;

   // block/wait for acknowledge, or until we time out;
   string answer;
   answer = "no answer";
   char ibuff[99]; // or what else, or use outer char array
   // ok to here return DEVICE_OK;
   ret = GetSerialAnswer(port_.c_str(), "\n", answer);
   
   LogMessage("GetPositionSteps GetSerialAnswer returns:", false);
   // ok to here return DEVICE_OK;
   sprintf_s(ibuff, "%i",ret);
   LogMessage("GetPositionSteps after GetSerialAnswer next is ret value", false);
   LogMessage(ibuff, false);
   LogMessage("GetPositionSteps Answer next", false);
   LogMessage(answer, false);
   LogMessage("after answer printed", false);
   
   //now parse out the answer.  Here is code for x,y, and z

	char x_str[20] = "";
	char y_str[20] = "";
	char z_str[20] = "";
	long int x_steps;
	long int y_steps;
	long int z_steps;

	size_t result;
	size_t found;
	found = answer.find("Z:");
	result = answer.copy(z_str, sizeof(z_str), found+2); 
 
	//get rid of the part of the string up to found-1
	answer = answer.substr(0,found-1);
	found = answer.find("Y:");
	result = answer.copy(y_str, sizeof(y_str), found+2); 

	//get rid of Y:  up to found-1
	answer = answer.substr(0,found-1);
	found = answer.find("X:");
	result = answer.copy(x_str, sizeof(x_str), found+2); 
	
	//now convert X,Y, and Z to long
	x_steps=0;
	y_steps=0;
	z_steps=0;
	
	x_steps = atol(x_str);
	y_steps = atol(y_str);
	z_steps = atol(z_str);

    //LogMessage(sprintf("M114 result (x,y,z)=(%d,%d,%d)\n", x_steps, y_steps, z_steps), false);
	char b[99];
	int f;
	f = sprintf(b, "M114 result (x,y,z)=(%d,%d,%d)\n", x_steps, y_steps, z_steps);
    LogMessage(b, false);


	if (answer.length() > 2 && answer.substr(0, 2).compare("ok") == 0)
    {
		//steps = atol(answer.substr(answer.find('Z')+2, 4));

		LogMessage("answer is ok:", false);
		LogMessage(answer, false);

		steps = z_steps;
		curSteps_ = steps;
		return DEVICE_OK;
    } else {
	   	LogMessage("answer is not ok", false);
		return DEVICE_OK;
    }
   
	return ERR_UNRECOGNIZED_ANSWER;
}

int GCodeZAxis::SetOrigin()
{
   return DEVICE_UNSUPPORTED_COMMAND;
}

int GCodeZAxis::GetLimits(double& /*min*/, double& /*max*/)
{
   return DEVICE_UNSUPPORTED_COMMAND;
}





///////////////////////////////////////////////////////////////////////////////
// GCodeXYStage implementation
// ~~~~~~~~~~~~~~~~~~~~~~~~~

GCodeXYStage::GCodeXYStage() : 
CXYStageBase<GCodeXYStage>(),
stepSize_um_(0.015),
posX_um_(0.0),
posY_um_(0.0),
busy_(false),
initialized_(false),
lowerLimit_(0.0),
upperLimit_(20000.0)
{
   InitializeDefaultErrorMessages();
}

GCodeXYStage::~GCodeXYStage()
{
   Shutdown();
}

void GCodeXYStage::GetName(char* Name) const
{
   CDeviceUtils::CopyLimitedString(Name, g_XYStageDeviceName);
}

int GCodeXYStage::Initialize()
{
   if (initialized_)
      return DEVICE_OK;

   // set property list
   // -----------------

   // Name
   int ret = CreateProperty(MM::g_Keyword_Name, g_XYStageDeviceName, MM::String, true);
   if (DEVICE_OK != ret)
      return ret;

   // Description
   ret = CreateProperty(MM::g_Keyword_Description, "Demo XY stage driver", MM::String, true);
   if (DEVICE_OK != ret)
      return ret;

   ret = UpdateStatus();
   if (ret != DEVICE_OK)
      return ret;

   initialized_ = true;

   return DEVICE_OK;
}

int GCodeXYStage::Shutdown()
{
   if (initialized_)
   {
      initialized_ = false;
   }
   return DEVICE_OK;
}
